
from collections import deque

# from matplotlib import pyplot as plt
import numpy as np


class PIDForceController():
    def __init__(self,kp,ki,kd,hybridpram=1,goalForce=-3,downward = True):
        self.Kp = kp
        self.Ki = ki
        self.kd = kd
        self.goalForce = goalForce
        self.error = 0                                 # 比例项
        self.sumerror = deque([0] * 150, maxlen=150)   # 积分项
        self.lasterror = 0                             # 微分项
        self.hybridAltha = hybridpram  # 混合控制系数

        self.downward = downward

    def _calPIDForce(self, current_force):
        force=current_force.copy()
        # if (abs(force[2]) < 10 ):
        #     force[2]=self.goalForce+1
        if force[2]<self.goalForce:
            # print("zforce=", np.round(force[2], 3))
            self.error = force[2] - self.goalForce  # 比例项
            # self.sumerror+=self.error
            self.sumerror.append(self.error)  # 积分项
            d_error = self.error - self.lasterror  # 微分项
            self.lasterror = self.error
            control_delta_pose = self.Kp * self.error + self.Ki * np.sum(self.sumerror) + self.kd * d_error
            return -self.hybridAltha*np.array([0, 0, control_delta_pose, 0, 0, 0])
        elif force[2]>-1 and self.downward:
            return np.array([0,0,-0.00003,0,0,0])
        else:
            return np.array([0,0,0,0,0,0])
        

if __name__ == '__main__':
    controller=PIDForceController(1/3000/6,1/10000/6,1/10000/6)
    force1=np.array([0,0,-11,0,0,0])
    force2=np.array([0,0,-5,0,0,0])
    print(controller._calPIDForce(force1))
    print(controller._calPIDForce(force2))
        